import numpy as np

# 64阶FIR滤波器系数
#  Fs = 1000Hz，Fpass = 10Hz，Fstop = 20Hz
coff_t_id = [
    0.001916484731361, 0.002659002665829, 0.003456972667297, 0.00430819089955,
    0.005209905285024, 0.006158824501662, 0.007151132811256, 0.008182510662111,
    0.009248160941438, 0.01034284068612, 0.01146089799526, 0.01259631382518,
    0.01374274828773, 0.01489359101696, 0.01604201511827, 0.01718103416787,
    0.01830356169075, 0.01940247251138, 0.020470665345, 0.02150112597737,
    0.02248699036914, 0.02342160701652, 0.02429859790345, 0.0251119173914,
    0.02585590841232, 0.02652535535611, 0.02711553307794, 0.02762225149096,
    0.0280418952567, 0.02837145813807, 0.02860857163757, 0.02875152760564,
    0.02879929456995, 0.02875152760564, 0.02860857163757, 0.02837145813807,
    0.0280418952567, 0.02762225149096, 0.02711553307794, 0.02652535535611,
    0.02585590841232, 0.0251119173914, 0.02429859790345, 0.02342160701652,
    0.02248699036914, 0.02150112597737, 0.020470665345, 0.01940247251138,
    0.01830356169075, 0.01718103416787, 0.01604201511827, 0.01489359101696,
    0.01374274828773, 0.01259631382518, 0.01146089799526, 0.01034284068612,
    0.009248160941438, 0.008182510662111, 0.007151132811256, 0.006158824501662,
    0.005209905285024, 0.00430819089955, 0.003456972667297, 0.002659002665829,
    0.001916484731361
        ]
coff_t_id = np.array(coff_t_id)


# 滤波器父类
class Base_FIR_Filter:
    def __init__(self, coff, rank):
        self.FIR_RANK = rank
        self.fir_state = np.zeros(self.FIR_RANK, dtype=np.float32)
        self.fir_state_index = -1
        self.COFF = coff

    def fir_filter(self, raw):
        # global fir_state_index, COFF, fir_state
        if self.fir_state_index == -1:
            self.fir_state[:] = raw
            self.fir_state_index = self.FIR_RANK - 1

        out = self.COFF[0] * raw
        i = self.fir_state_index + 1
        if i < self.FIR_RANK:
            out = out + sum(self.fir_state[i:self.FIR_RANK] * self.COFF[1:self.FIR_RANK - self.fir_state_index])

        out = out + sum(self.COFF[self.FIR_RANK - self.fir_state_index:] * self.fir_state[0:self.fir_state_index + 1])

        self.fir_state[self.fir_state_index] = raw
        if self.fir_state_index == 0:
            self.fir_state_index = self.FIR_RANK - 1
        else:
            self.fir_state_index = self.fir_state_index - 1

        return out


# FIR滤波器子类，用于T-Id转移曲线
class T_Id_Filter(Base_FIR_Filter):
    def __init__(self):
        super(T_Id_Filter, self).__init__(coff_t_id, rank=64)
